#include <Arduino.h>
#include <WIFI.h>
#include <WIFIServer.h>

#include <string>

#include "PWM_control.hpp"
// #include "pid.hpp"

// 舵机 ---------------------------------------------------------------------------------------------
// 舵机 50Hz 的频率，20ms 的周期
constexpr int servo_freq = 50;
constexpr int servo_channel = 8;
// 舵机分辨率设置为 12，用 4096 的数值来映射角度
constexpr int servo_resolution =  12;
// 定义舵机 PWM 控制引脚
constexpr int servo_pin = 13;
// servo 103 中心 最左角113 最右角93 左加右减
constexpr int degree_mid = 103;
constexpr int degree_left = 113;
constexpr int degree_right = 93;
Servo servo(servo_pin, servo_channel, servo_resolution, degree_left, degree_right);

// 电机 ---------------------------------------------------------------------------------------------
// 电机 17000Hz 的频率，58.8us 的周期
constexpr int motor_freq = 1000;
constexpr int left_channel = 0;
constexpr int right_channel = 1;
// 电机分辨率设置为 13，用 8192 的数值来映射角度
constexpr int motor_resolution =  13;
// 左电机
constexpr int left_pin = 14;
constexpr int left_dir_a_pin = 4;
constexpr int left_dir_b_pin = 16;
// 右电机
constexpr int right_pin = 12;
constexpr int right_dir_a_pin = 2;
constexpr int right_dir_b_pin = 15;
motor left_motor(left_pin, left_channel, motor_resolution, motor_freq, left_dir_a_pin, left_dir_b_pin);
motor right_motor(right_pin, right_channel, motor_resolution, motor_freq, right_dir_a_pin, right_dir_b_pin);

// // 编码器 --------------------------------------------------------------------------------------------
// pid left_pid(pid::PID_MODE::POSITION_PID, 10, 0.1, 5, -(1 << 13), (1 << 13));
// pid right_pid(pid::PID_MODE::POSITION_PID, 10, 0.1, 5, -(1 << 13), (1 << 13));
// constexpr int left_encoder_a = 34;
// constexpr int left_encoder_b = 35;
// int left_count = 0;

// constexpr int right_encoder_a = 32;
// constexpr int right_encoder_b = 33;
// int right_count = 0;

// void left_encoder_A(void)
// {
//     left_count++;
// }
// void left_encoder_B(void)
// {
//     left_count--;
// }
// void right_encoder_A(void)
// {
//     right_count++;
// }
// void right_encoder_B(void)
// {
//     right_count--;
// }

// void encoder_init()
// {
//     pinMode(left_encoder_a, INPUT);
//     pinMode(left_encoder_b, INPUT);
//     pinMode(right_encoder_a, INPUT);
//     pinMode(right_encoder_b, INPUT);
//     attachInterrupt(left_encoder_a, left_encoder_A, RISING);
//     attachInterrupt(left_encoder_b, left_encoder_B, RISING);
//     attachInterrupt(right_encoder_a, right_encoder_A, RISING);
//     attachInterrupt(right_encoder_b, right_encoder_B, RISING);
// }

// // 定时器 --------------------------------------------------------------------------------------------
// hw_timer_t *timer = NULL;

// // 定时器中断处理函数
// void car_control_interrupt()
// {
//     left_pid._now = left_count;
//     left_count = 0;
//     right_pid._now = right_count;
//     right_count = 0;
// }

// void timer_init()
// {
//     // 初始化定时器
//     timer = timerBegin(0, 80, true);

//     // 配置定时器
//     timerAttachInterrupt(timer, car_control_interrupt, true);

//     timerAlarmWrite(timer, 10000, true);

//     // 启动定时器
//     timerAlarmEnable(timer);
// }

// WIFI -------------------------------------------------------------------------------------------
const char* ssid = "esp_test";
const char* password = "xcz12345678";
const char* ip = "192.168.22.1";
const short port = 8080;
// tcp对象
WiFiServer server(port);

void internet_init()
{
    WiFi.mode(WIFI_MODE_AP);
    WiFi.softAP(ssid, password);
    IPAddress ipaddr;
    ipaddr.fromString(ip);
    WiFi.softAPConfig(ipaddr, ipaddr, WiFi.softAPSubnetMask());

    server.begin();
}

// 初始化 --------------------------------------------------------------------------------------------
void setup()
{
    Serial.begin(9600);

    internet_init();
    servo.init();
    servo.degree_control(degree_mid);
    
    left_motor.init();
    right_motor.init();
}

void deserialize(std::string& buffer, int& degree, int& speed)
{
    int pos = buffer.find('x');
    degree = ((buffer[pos + 3] - '0') * 100) + ((buffer[pos + 4] - '0') * 10) + ((buffer[pos + 5] - '0') * 1);
    if (buffer[pos + 2] == '-')
        degree = -degree;
    speed = ((buffer[pos + 10] - '0') * 100) + ((buffer[pos + 11] - '0') * 10) + ((buffer[pos + 12] - '0') * 1);
    if (buffer[pos + 9] == '-')
        speed = -speed;
    int end = buffer.find('x', pos + 1);
    buffer.erase(0, end);
}

void range_map_PWM(int& speed, int& degree)
{
    speed = map(speed, -100, 100, -5000, 5000);
    degree = map(degree, -100, 100, 113, 93);
}

void loop()
{
    static int begin = millis();
    WiFiClient client = server.available();
    if (client)
    {
        client.setTimeout(0);
        std::string read_buffer;
        int speed = 0, degree = degree_mid;
        while (client.connected())
        {
            begin = millis();
            // 接收遥控器信息
            if (client.available())
            {
                char buffer[20] = { 0 };
                int n = client.readBytes(buffer, 18);
                buffer[n] = '\0';
                read_buffer += buffer;
                deserialize(read_buffer, degree, speed);
                range_map_PWM(speed, degree);
                // Serial.print(degree);
                // Serial.print(' ');
                // Serial.println(speed);
            }
            // 控制小车
            servo.degree_control(degree);
            left_motor.out(speed);
            right_motor.out(speed);
            int end = millis();
            Serial.println(end - begin);
            begin = end;
        }
        client.stop();
    }
}
